#include "app_cfg.h"
#include "app_type.h"

#include "interfaces.h"
#include "tool/buffer/buffer.h"

#include "framework/vsfsm/vsfsm.h"
#include "framework/vsftimer/vsftimer.h"

#define TEST_IIC_COMPLETE			(VSFSM_EVT_LOCAL + 3)

static struct vsfsm_t test_sm;
static struct vsfsm_pt_t test_pt;

struct test_t
{
	uint8_t index;
	uint8_t addr;

	uint8_t read_buf[20];
	uint8_t read_len;

	uint8_t write_buf[20];
	uint8_t write_len;

	int8_t oper_ok;
} static test;


static void test_i2c_callback(void *param, int32_t read_len,
								int32_t write_len)
{	
	if ((read_len == test.read_len) &&
		(write_len == test.write_len))
	{
		test.oper_ok = 0;
	}
	else
	{
		test.oper_ok = -1;
	}
	vsfsm_post_evt_pending(&test_sm, TEST_IIC_COMPLETE);
}

#define I2C_XFER(read_first) \
	do{interfaces->i2c.xfer_nb(test.index, test.addr, read_first, test.read_buf, test.read_len, test.write_buf, test.write_len);}while(0)
#define I2C_READ(sm, cric) \
	do{test.write_len = 0;\\
	interfaces->i2c.xfer_nb(test.index, test.addr, read_first, test.read_buf, test.read_len, NULL, 0);}while(0)
#define I2C_WRITE(sm, cric) \
	do{pm->read_len = 0;\
	interfaces->i2c.xfer_nb(test.index, test.addr, 0, NULL, 0, test.write_buf, test.write_len);}while(0)

static vsf_err_t test_thread(struct vsfsm_pt_t *pt, vsfsm_evt_t evt)
{
	vsfsm_pt_begin(pt);
	
	// LED TEST
	interfaces->gpio.clear(REG_0R_PORT, 1 << REG_0R_POIN);
	interfaces->gpio.clear(REG_1R_PORT, 1 << REG_1R_POIN);
	interfaces->gpio.clear(REG_2R_PORT, 1 << REG_2R_POIN);
	interfaces->gpio.clear(REG_3R_PORT, 1 << REG_3R_POIN);
	vsfsm_pt_delay(pt, 500);
	interfaces->gpio.set(REG_0R_PORT, 1 << REG_0R_POIN);
	interfaces->gpio.set(REG_1R_PORT, 1 << REG_1R_POIN);
	interfaces->gpio.set(REG_2R_PORT, 1 << REG_2R_POIN);
	interfaces->gpio.set(REG_3R_PORT, 1 << REG_3R_POIN);
	interfaces->gpio.clear(REG_0G_PORT, 1 << REG_0G_POIN);
	interfaces->gpio.clear(REG_1G_PORT, 1 << REG_1G_POIN);
	interfaces->gpio.clear(REG_2G_PORT, 1 << REG_2G_POIN);
	interfaces->gpio.clear(REG_3G_PORT, 1 << REG_3G_POIN);
	vsfsm_pt_delay(pt, 500);
	interfaces->gpio.set(REG_0G_PORT, 1 << REG_0G_POIN);
	interfaces->gpio.set(REG_1G_PORT, 1 << REG_1G_POIN);
	interfaces->gpio.set(REG_2G_PORT, 1 << REG_2G_POIN);
	interfaces->gpio.set(REG_3G_PORT, 1 << REG_3G_POIN);
	interfaces->gpio.clear(REG_0B_PORT, 1 << REG_0B_POIN);
	interfaces->gpio.clear(REG_1B_PORT, 1 << REG_1B_POIN);
	interfaces->gpio.clear(REG_2B_PORT, 1 << REG_2B_POIN);
	interfaces->gpio.clear(REG_3B_PORT, 1 << REG_3B_POIN);
	vsfsm_pt_delay(pt, 500);
	interfaces->gpio.set(REG_0B_PORT, 1 << REG_0B_POIN);
	interfaces->gpio.set(REG_1B_PORT, 1 << REG_1B_POIN);
	interfaces->gpio.set(REG_2B_PORT, 1 << REG_2B_POIN);
	interfaces->gpio.set(REG_3B_PORT, 1 << REG_3B_POIN);

#if 0
	// MOTOR TEST
	interfaces->gpio.set(MOTOR_0_PORT, 1 << MOTOR_0_POIN);
	vsfsm_pt_delay(pt, 200);
	interfaces->gpio.clear(MOTOR_0_PORT, 1 << MOTOR_0_POIN);
	vsfsm_pt_delay(pt, 800);
	interfaces->gpio.set(MOTOR_1_PORT, 1 << MOTOR_1_POIN);
	vsfsm_pt_delay(pt, 200);
	interfaces->gpio.clear(MOTOR_1_PORT, 1 << MOTOR_1_POIN);
	vsfsm_pt_delay(pt, 800);
	interfaces->gpio.set(MOTOR_2_PORT, 1 << MOTOR_2_POIN);
	vsfsm_pt_delay(pt, 200);
	interfaces->gpio.clear(MOTOR_2_PORT, 1 << MOTOR_2_POIN);
	vsfsm_pt_delay(pt, 800);
	interfaces->gpio.set(MOTOR_3_PORT, 1 << MOTOR_3_POIN);
	vsfsm_pt_delay(pt, 200);
	interfaces->gpio.clear(MOTOR_3_PORT, 1 << MOTOR_3_POIN);
	vsfsm_pt_delay(pt, 800);
#endif
	
	
	// iic	
	test.index = SENSOR_I2C;
	// bmp280	
	test.addr = 0x76;	
	test.write_buf[0] = 0xd0;
	test.write_len = 1;
	test.read_len = 1;
	I2C_XFER(0);
	vsfsm_pt_wfe(pt, TEST_IIC_COMPLETE);
	if (test.oper_ok)
		return VSFERR_FAIL;
	if (test.read_buf[0] != 0x58)
		return VSFERR_FAIL;
	interfaces->gpio.clear(REG_3R_PORT, 1 << REG_3R_POIN);
	
	// hmc5883l
	test.addr = 0x1e;		
	test.write_buf[0] = 0x04;
	test.write_len = 1;
	test.read_len = 1;
	I2C_XFER(0);
	vsfsm_pt_wfe(pt, TEST_IIC_COMPLETE);
	if (test.oper_ok)
		return VSFERR_FAIL;
	interfaces->gpio.clear(REG_3G_PORT, 1 << REG_3G_POIN);
	
	// mpu6050
	test.addr = 0x68;		
	test.write_buf[0] = 117;
	test.write_len = 1;
	test.read_len = 1;
	I2C_XFER(0);
	vsfsm_pt_wfe(pt, TEST_IIC_COMPLETE);
	if (test.oper_ok)
		return VSFERR_FAIL;
	if (test.read_buf[0] != 0x68)
		return VSFERR_FAIL;
	interfaces->gpio.clear(REG_3B_PORT, 1 << REG_3B_POIN);
	
	vsfsm_pt_end(pt);
	
	return VSFERR_NONE;
}


static struct vsfsm_state_t *test_handler(struct vsfsm_t *sm, vsfsm_evt_t evt)
{
	vsf_err_t err;
	
	switch (evt)
	{
	case VSFSM_EVT_ENTER:
		break;
	case VSFSM_EVT_INIT:
		interfaces->gpio.init(0);
		interfaces->gpio.init(1);
		interfaces->gpio.init(2);
		interfaces->gpio.init(3);
		interfaces->gpio.init(4);
		interfaces->gpio.init(5);
		interfaces->gpio.config(REG_0R_PORT, 1 << REG_0R_POIN, 1 << REG_0R_POIN, 1 << REG_0R_POIN, 1 << REG_0R_POIN);
		interfaces->gpio.config(REG_0G_PORT, 1 << REG_0G_POIN, 1 << REG_0G_POIN, 1 << REG_0G_POIN, 1 << REG_0G_POIN);
		interfaces->gpio.config(REG_0B_PORT, 1 << REG_0B_POIN, 1 << REG_0B_POIN, 1 << REG_0B_POIN, 1 << REG_0B_POIN);
		interfaces->gpio.config(REG_1R_PORT, 1 << REG_1R_POIN, 1 << REG_1R_POIN, 1 << REG_1R_POIN, 1 << REG_1R_POIN);
		interfaces->gpio.config(REG_1G_PORT, 1 << REG_1G_POIN, 1 << REG_1G_POIN, 1 << REG_1G_POIN, 1 << REG_1G_POIN);
		interfaces->gpio.config(REG_1B_PORT, 1 << REG_1B_POIN, 1 << REG_1B_POIN, 1 << REG_1B_POIN, 1 << REG_1B_POIN);
		interfaces->gpio.config(REG_2R_PORT, 1 << REG_2R_POIN, 1 << REG_2R_POIN, 1 << REG_2R_POIN, 1 << REG_2R_POIN);
		interfaces->gpio.config(REG_2G_PORT, 1 << REG_2G_POIN, 1 << REG_2G_POIN, 1 << REG_2G_POIN, 1 << REG_2G_POIN);
		interfaces->gpio.config(REG_2B_PORT, 1 << REG_2B_POIN, 1 << REG_2B_POIN, 1 << REG_2B_POIN, 1 << REG_2B_POIN);
		interfaces->gpio.config(REG_3R_PORT, 1 << REG_3R_POIN, 1 << REG_3R_POIN, 1 << REG_3R_POIN, 1 << REG_3R_POIN);
		interfaces->gpio.config(REG_3G_PORT, 1 << REG_3G_POIN, 1 << REG_3G_POIN, 1 << REG_3G_POIN, 1 << REG_3G_POIN);
		interfaces->gpio.config(REG_3B_PORT, 1 << REG_3B_POIN, 1 << REG_3B_POIN, 1 << REG_3B_POIN, 1 << REG_3B_POIN);
		
		interfaces->gpio.clear(MOTOR_0_PORT, 1 << MOTOR_0_POIN);
		interfaces->gpio.clear(MOTOR_1_PORT, 1 << MOTOR_1_POIN);
		interfaces->gpio.clear(MOTOR_2_PORT, 1 << MOTOR_2_POIN);
		interfaces->gpio.clear(MOTOR_3_PORT, 1 << MOTOR_3_POIN);
		interfaces->gpio.config(MOTOR_0_PORT, 1 << MOTOR_0_POIN, 1 << MOTOR_0_POIN, 1 << MOTOR_0_POIN, 0);
		interfaces->gpio.config(MOTOR_1_PORT, 1 << MOTOR_1_POIN, 1 << MOTOR_1_POIN, 1 << MOTOR_1_POIN, 0);
		interfaces->gpio.config(MOTOR_2_PORT, 1 << MOTOR_2_POIN, 1 << MOTOR_2_POIN, 1 << MOTOR_2_POIN, 0);
		interfaces->gpio.config(MOTOR_3_PORT, 1 << MOTOR_3_POIN, 1 << MOTOR_3_POIN, 1 << MOTOR_3_POIN, 0);
		
		interfaces->i2c.init(SENSOR_I2C);
		interfaces->i2c.config(SENSOR_I2C, 400, NULL, &test_i2c_callback);
		
	default:
		err = test_thread(&test_pt, evt);
		if (err <= 0)
		{
			// TODO: error deal
		}
		break;
	}
	
	return NULL;
}

void test_init(void)
{
	test_sm.init_state.evt_handler = test_handler;
	test_pt.sm = &test_sm;
	test_pt.thread = &test_thread;
	vsfsm_init(&test_sm);
}